
(* ====================================================================== *)
(*  CFSQP Interface                                                       *)
(* ====================================================================== *)

open Common

module R = Random
module A = Array
module A' = Array'
module C = Callback

let _ = R.self_init()

(* -------------------------------------------------------------------------- *)
(*  Test                                                                      *)
(* -------------------------------------------------------------------------- *)

external incr_arr_aux : float array -> int -> float array = "incr_arr_"

let incr_arr a = incr_arr_aux a (A.length a)

(* -------------------------------------------------------------------------- *)
(*  Callbacks                                                                 *)
(* -------------------------------------------------------------------------- *)

let obj_fun_callback_name = "caml_obj_fun"
let constr_fun_callback_name = "caml_constr_fun"

external cfsqp_minimize :
  int -> int -> float array -> float array -> float array -> float * float array
  = "cfsqp_minimize_"

(* -------------------------------------------------------------------------- *)
(*  Minimize                                                                  *)
(* -------------------------------------------------------------------------- *)

(** given two arrays of length n, return an array of length
    n with random values between those of the corresponding arrays *)
let get_random min max =
  let minl = A.to_list min in
  let maxl = A.to_list max in
  let random_split lo hi =
    lo +. R.float (hi -. lo) in
    A.of_list(map2 random_split minl maxl)

let minimize_once obj_fun constr_fun nconstr min max =
  let _ = C.register obj_fun_callback_name obj_fun in
  let _ = C.register constr_fun_callback_name constr_fun in
  let init = get_random min max in
    cfsqp_minimize (A.length min) nconstr min max init

let repeat = ref 30
(** number of iterations *)

let amin xs = 
  let n = A.length xs in
  if n = 0 then failwith "amin: empty array" else
  begin 
    let min = ref (A.get xs 0) in
    for i = 0 to n - 1 do
      let v = A.get xs i in
        if fst v < fst !min then min := v
    done;
    !min
  end

let minimize obj_fun constr_fun nconstr min max =
  let times = !repeat in
  let arr = A.create times (0.0,[||]) in
    begin
      for i = 0 to times - 1 do
        let res = minimize_once obj_fun constr_fun nconstr min max in
          A.set arr i res
      done;
      amin arr
    end

let simple_min obj_fun min max =
  minimize obj_fun (fun n x -> failwith "simple_min") 0 min max

let very_simple_min obj_fun args min max =
  simple_min obj_fun (A.create args min) (A.create args max)

(* -------------------------------------------------------------------------- *)
(*  Maximize                                                                  *)
(* -------------------------------------------------------------------------- *)

let negate_fun f x = -. (f x)

let maximize obj_fun constr_fun nconstr min max =
  let v,vs = minimize (negate_fun obj_fun) constr_fun nconstr min max in
    -. v, vs

let simple_max obj_fun min max =
  maximize obj_fun (fun n x -> failwith "simple_max") 0 min max

let very_simple_max obj_fun args min max =
  simple_max obj_fun (A.create args min) (A.create args max)
